/**
 * @FilePath: \ros2\src\agv_control\chassis_controller\include\chassis_controller\chassis_controller.hpp
 * @Date: 2025-03-07 19:20:47
 * @Description:  
 * @Author: haibo
 * @Version: 1.0.0
 * @LastEditors: haibo haibo.yang@lgmgim.cn
 * @LastEditTime: 2025-03-20 19:41:18
 * @Copyright (c) 2025 by 临工智能信息科技有限公司, All Rights Reserved. 
**/

#ifndef CHASSIS_CONTROLLER__CHASSIS_CONTROLLER_HPP_
#define CHASSIS_CONTROLLER__CHASSIS_CONTROLLER_HPP_

#include <memory>
#include <string>
#include <vector>
#include <queue>

#include "controller_interface/controller_interface.hpp"
// #include "chassis_controller/chassis_controller_parameters.hpp"
#include "chassis_controller/visibility_control.h"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/realtime_buffer.hpp"
#include "realtime_tools/realtime_publisher.hpp"
#include "chassis_controller/odometry.h"
#include "tf2_msgs/msg/tf_message.hpp"
#include <agv_srvs/srv/output_interface.hpp>
#include <agv_srvs/srv/motor_enable.hpp>
#include <agv_msgs/msg/output_port.hpp>
#include <agv_msgs/msg/chassis.hpp>
#include <agv_msgs/msg/do_steer_dir.hpp>
#include <agv_msgs/msg/gross_load.hpp>
#include <agv_msgs/msg/steer_dir_feed_back.hpp>
#include <agv_msgs/msg/odometry_lite.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <agv_msgs/msg/read_in_puts_new.hpp>
#include <agv_msgs/msg/pair_type_int.hpp>
#include "agv_msgs/msg/drivers_info.hpp"
#include <geometry_msgs/msg/twist_stamped.hpp>

#include <chassis_controller/odometry_multi_pivots.h>
#include <chassis_controller/speed_limiter.h>
#include "chassis_controller/kinematics.h"
#include <chassis_controller/pid.h>
#include <chassis_controller/odometry_compensation.h>
#include "chassis_controller/current_monitor.h"


namespace chassis_controller
{

class ChassisController : public controller_interface::ControllerInterface
{
public:
  CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
  ChassisController();

  CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::CallbackReturn on_init() override;

  CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::InterfaceConfiguration command_interface_configuration() const override;

  CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::InterfaceConfiguration state_interface_configuration() const override;

  CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::CallbackReturn on_configure(
    const rclcpp_lifecycle::State & previous_state) override;

  CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::CallbackReturn on_activate(
    const rclcpp_lifecycle::State & previous_state) override;

  CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::CallbackReturn on_deactivate(
    const rclcpp_lifecycle::State & previous_state) override;

  CHASSIS_CONTROLLER__VISIBILITY_PUBLIC
  controller_interface::return_type update(
    const rclcpp::Time & time, const rclcpp::Duration & period) override;

  // TODO(anyone): replace the state and command message types
  using ControllerTwistReferenceMsg = geometry_msgs::msg::TwistStamped;
  using ControllerStateMsgOdom = agv_msgs::msg::OdometryLite;
  using ControllerStateMsgTf = tf2_msgs::msg::TFMessage;
protected:
  enum joint_type {TRACTION = 0 ,STEER =1 , ENCODER = 2};
  
  controller_interface::CallbackReturn configure_side(
    const enum joint_type & wheel_kind,
    const std::string & joint_name,
    GenericHandle & registered_handles);

  // std::shared_ptr<chassis_controller::ParamListener> param_listener_;
  // chassis_controller::Params params_;

  // Command subscribers and Controller State publisher
  std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::AGVEvent>>  event_pub_;
  std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::OdometryLite>> odom_publisher_;
  std::shared_ptr<rclcpp::Publisher<tf2_msgs::msg::TFMessage>> tf_odom_publisher_;
  std::shared_ptr<rclcpp::Publisher<agv_msgs::msg::SteerDirFeedBack>> streer_publisher_;
  std::shared_ptr<realtime_tools::RealtimePublisher<agv_msgs::msg::OdometryLite>> rt_odom_state_publisher_;
  std::shared_ptr<realtime_tools::RealtimePublisher<tf2_msgs::msg::TFMessage>> rt_tf_odom_state_publisher_;
  std::shared_ptr<realtime_tools::RealtimePublisher<agv_msgs::msg::SteerDirFeedBack>> rt_streer_publisher_;

  rclcpp::Subscription<ControllerTwistReferenceMsg>::SharedPtr ref_subscriber_twist_;
  rclcpp::Subscription<agv_msgs::msg::ReadInPutsNew>::SharedPtr sub_readinputnew_;
  rclcpp::Subscription<agv_msgs::msg::DoSteerDir>::SharedPtr steer_direction_;
  rclcpp::Subscription<agv_msgs::msg::GrossLoad>::SharedPtr sub_gross_load_;
  rclcpp::Subscription<agv_msgs::msg::DriversInfo>::SharedPtr sub_driver_info_;
  // rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_IO_;

  rclcpp::Client<agv_srvs::srv::OutputInterface>::SharedPtr output_client_;
  rclcpp::Client<agv_srvs::srv::MotorEnable>::SharedPtr motor_enable_client_;

  realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerTwistReferenceMsg>> input_ref_;
  rclcpp::Duration ref_timeout_ = rclcpp::Duration::from_seconds(0.0);  // 0ms

private:
  std::vector<std::future<bool>>  futures_;
  bool                            joints_enable_check_ = false;
  bool                            joints_enable_ = true;
  rclcpp::Time                    brake_operate_time_;
  bool                            ready_move_ = true;
  double                          brake_delay_time_ = 0.5;
  double                          brake_time_ = 0.5;    
  bool                            enable_brake_control_ = false;
  bool                            use_ekf_with_imu_ = false;
  bool                            use_pdo_timestamp_ = false;
  
  monitor_param monitor_param_;
  CurrentMonitor current_monitor_;
  agv_msgs::msg::AGVEvent event_msg_;
  double event_pub_time_;
  struct vehicle_status
  {
    bool emergency_stop;
    bool stop;
    bool delay_stop;
    bool slowdown;   
  } vehicle_status_;

  struct DynamicPivotParams
  {
    std::vector<double> wheel_diameter;
    std::vector<double> radius_ringmount;
    std::vector<double> steer_offset;
    std::vector<double> offset_x;
    std::vector<double> offset_y;
    // std::vector<double> left_wheel_diameter;
    // std::vector<double> right_wheel_diameter; 
  } dynamic_pivot_params_;

  bool braker_out_last_;

  std::string name_;
  int pivot_num_;
  int joint_num_;
  int moter_num_;
  int encoder_joint_num_;
  int chassis_type_;
  int braker_index_;
  int filter_window_;
  bool reset_button_;
  uint64_t steer_cmd_id_; 
  pid_param steer_pid_param_;
  std::vector<std::string> moter_names_;

  Odometry *odometryPtr_;
  Kinematics *kinematicsPtr_;
  DynamicOdom dynamic_odom_;
  double gross_load_ = 0.0;
  double dynamic_factor_ = 1.0;
  double odom_factor_ = 1.0;

  /// Hardware handles:
  std::vector<joint_type> joint_type_vector_; 
  std::vector<std::string> encoder_joint_names_;  
  std::vector<Spivot_value> pivot_value_;
  std::vector<Spivot_setpoint> pivot_setpoint_;
  std::vector<Spivot_param> pivot_param_;

  void pubError();
  
  std::vector<double> pose_covariance_diagonal_ = {0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0};
  std::vector<double> twist_covariance_diagonal_ = {0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0};

  rclcpp::Duration publish_period_;
  rclcpp::Time last_state_publish_time_;
  bool open_loop_;
  bool standstill_;

  /// Timeout to consider cmd_vel commands old:
  double cmd_vel_timeout_;

  /// Whether to allow multiple publishers on cmd_vel topic or not:
  bool allow_multiple_cmd_vel_publishers_;

  /// Frame to use for the robot base:
  std::string base_frame_id_;
  /// Frame to use for odometry and odom tf:
  std::string odom_frame_id_;
  /// Whether to publish odometry to tf or not:
  bool enable_odom_tf_;

  /// If the diffrence between desired steer angle its joint state is larger
  /// than angle_tolerance, stop wheel from turning
  double angle_tolerance_;

  /// Speed limiters:
  double last1_cmd_liner_ = 0.0;
  double last0_cmd_liner_ = 0.0;
  double last1_cmd_angular_ = 0.0;
  double last0_cmd_angular_ = 0.0;
  SpeedLimiter limiter_lin_;
  SpeedLimiter limiter_ang_;
  SpeedLimiter steer_limiter_;

  double steer_offset1_,steer_offset2_,steer_offset3_,steer_offset4_,steer_offset5_,steer_offset6_,steer_offset7_,steer_offset8_;
  double offset_y_;
  double wheel_diameter_multiplier_;
  double pivot_offset_multiplier_;
  double ring_mount_multiplier_;
  double left_wheel_radius_multiplier_;
  double right_wheel_radius_multiplier_;
  std::vector<double> steer_offset_addend;
  int hardware_interface_count_;
  bool drivers_state_ready_;
  
  std::mutex enable_mutex_;
  rclcpp::TimerBase::SharedPtr activation_timer_;
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
  
  inline auto& get_clock() { return *(get_node()->get_clock()); }
  
  bool config();
  void reference_callback(const std::shared_ptr<ControllerTwistReferenceMsg> msg);
  void updatePivotValue();
  void updateOdometry(const rclcpp::Time &time);
  void updateCommand(const rclcpp::Time &time, const rclcpp::Duration &period);
  void readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg);
  void steerDirCallback(const agv_msgs::msg::DoSteerDir &steer_dir);
  void drivers_info_callback(const agv_msgs::msg::DriversInfo &driver_info);
  void brakerControl();
  bool enable_motor_devices(bool enable);
  rcl_interfaces::msg::SetParametersResult parametersCallback(const std::vector<rclcpp::Parameter> & parameters);
  void update_dynamic_parameters(); 
};

}  // namespace chassis_controller

#endif  // CHASSIS_CONTROLLER__CHASSIS_CONTROLLER_HPP_
